function [J, D] = distortimage(intrinsics, I, noiseStdDev, fillValues)
%DISTORTIMAGE 计算畸变位移场，并在图像中加入畸变
%
% Tips
% # 当使用imwarp函数应用本函数输出的位移场D时，为获得正确的畸变，需要取负值，即imwarp(I, -D)
%
% Input Arguments
% # intrinsics: cameraIntrinsics对象，包含图像尺寸、主点、焦距、Skew、径向畸变系数、切向畸变系数等域
% # I: 矩阵，原始灰度图像，默认为空，此时仅计算畸变位移场
% # noiseStdDev: 标量，在位移场中附加高斯噪声的标准差，空或者NaN表示不加噪声，单位像素，默认为空
% # fillValues: 标量，畸变后图像边界未赋值部分的填充值，默认为255（白色）
%
% Output Arguments
% # D: 包含2页的三维矩阵，前两维尺寸与I相同，分别为X、Y方向I的各像素点的畸变位移
% # J: 与I相同尺寸的矩阵，畸变后的灰度图像
% 
% References:
% #《应用导航算法工程基础 》 “相机畸变模型”

if nargin < 2
    I = [];
end
if nargin < 3
    noiseStdDev = [];
end
if nargin < 4
    fillValues = 255;
end

if length(intrinsics.RadialDistortion) == 2
    radialDist = [intrinsics.RadialDistortion 0];
else
    radialDist = intrinsics.RadialDistortion;
end

m = intrinsics.ImageSize(1, 2);
n = intrinsics.ImageSize(1, 1);
assert(isequal(size(I), [m n]));

% add distortion
if all(radialDist==0) && all(intrinsics.TangentialDistortion==0)
    D = zeros(m, n, 2);
else
    y = repmat(((1:m)'-intrinsics.PrincipalPoint(1, 2)) / intrinsics.FocalLength(1, 2), 1, n);
    x = (repmat((1:n)-intrinsics.PrincipalPoint(1, 1), m, 1)-intrinsics.Skew*y) / intrinsics.FocalLength(1, 1);
    xSq = x .^ 2;
    ySq = y .^ 2;
    rSq = xSq + ySq;
    D(:, :, 1) = x;
    D(:, :, 2) = y;
    if any(radialDist~=0)
        rQd = rSq .^ 2;
        D(:, :, 1) = D(:, :, 1) + x.*(radialDist(1, 1)*rSq + radialDist(1, 2)*rQd + radialDist(1, 3)*rSq.*rQd);
        D(:, :, 2) = D(:, :, 2) + y.*(radialDist(1, 1)*rSq + radialDist(1, 2)*rQd + radialDist(1, 3)*rSq.*rQd);
        clearvars rQd
    end
    if any(intrinsics.TangentialDistortion~=0)
        xy = x .* y;
        clearvars x y
        D(:, :, 1) = D(:, :, 1) + 2*intrinsics.TangentialDistortion(1, 1)*xy + intrinsics.TangentialDistortion(1, 2)*(rSq + 2*xSq);
        D(:, :, 2) = D(:, :, 2) + 2*intrinsics.TangentialDistortion(1, 2)*xy + intrinsics.TangentialDistortion(1, 1)*(rSq + 2*ySq);
        clearvars xy xSq ySq rSq
    else
        clearvars x y xSq ySq rSq
    end
    D(:, :, 1) = D(:, :, 1)*intrinsics.FocalLength(1, 1) + D(:, :, 2)*intrinsics.Skew + intrinsics.PrincipalPoint(1, 1) - repmat(1:n, m, 1);
    D(:, :, 2) = D(:, :, 2)*intrinsics.FocalLength(1, 2) + intrinsics.PrincipalPoint(1, 2) - repmat((1:m)', 1, n);
end

% add noise
if ~isempty(noiseStdDev) && ~isnan(noiseStdDev)
    D = D + noiseStdDev*randn(size(D));
end

% remap the image
if ~isempty(I)
    if isfloat(I)
        J = imwarp(I, -D, 'FillValues', fillValues);
    else
        J = imwarp(double(I), -D, 'FillValues', fillValues);
    end
end
end